#include "mesh.hpp"
#include "geometry.hpp"
#include "util.hpp"
#include "mesh_testFunctions.hpp"

#include <catch2/catch_test_macros.hpp>
#include <catch2/catch_approx.hpp>
#include <vector>
#include <utility>

using namespace hms;

using Catch::Approx;


TEST_CASE( "Two vectors are sorted by pseudoangle" )
{
	/* pseudoangle implementation has a discontinuity at a leftward horizontal 
	 * vector, just like atan2(). thus everything from 9 o'clock 
	 * counter-clockwise is tested.
	 */
	using hms::pseudoangle;
	SECTION( "passing 2D vectors directly" )
	{
		REQUIRE(
			pseudoangle( Vector2s {-2,-hms::epsilon} ) <
			pseudoangle( Vector2s {-2,-1} )
		);
		REQUIRE(
			pseudoangle( Vector2s {-2,-1} ) <
			pseudoangle( Vector2s {-2,-2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {-2,-2} ) <
			pseudoangle( Vector2s {-1,-2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {-1,-2} ) <
			pseudoangle( Vector2s { 0,-2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {0,-2} ) <
			pseudoangle( Vector2s {1,-2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {1,-2} ) <
			pseudoangle( Vector2s {2,-2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {2,-2} ) <
			pseudoangle( Vector2s {2,-1} )
		);
		REQUIRE(
			pseudoangle( Vector2s {2,-1} ) <
			pseudoangle( Vector2s {2, 0} )
		);
		REQUIRE(
			pseudoangle( Vector2s {2, 0} ) <
			pseudoangle( Vector2s {2, 1} )
		);
		REQUIRE(
			pseudoangle( Vector2s {2, 1} ) <
			pseudoangle( Vector2s {2, 2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {2, 2} ) <
			pseudoangle( Vector2s {1, 2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {1, 2} ) <
			pseudoangle( Vector2s {0, 2} )
		);
		REQUIRE(
			pseudoangle( Vector2s { 0, 2} ) <
			pseudoangle( Vector2s {-1, 2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {-1, 2} ) <
			pseudoangle( Vector2s {-2, 2} )
		);
		REQUIRE(
			pseudoangle( Vector2s {-2, 2} ) <
			pseudoangle( Vector2s {-2, 1} )
		);
		REQUIRE(
			pseudoangle( Vector2s {-2, 1} ) <
			pseudoangle( Vector2s {-2, hms::epsilon} )
		);
	}
	SECTION( "accessing vertices through mesh" )
	{
		scalar ep { hms::epsilon };
		Matrix2Xs verts (2, 16);
		verts <<
			-2, -2, -2, -1,  0,  1,  2,  2, 2, 2, 2, 1, 0, -1, -2, -2,
			-ep,-1, -2, -2, -2, -2, -2, -1, 0, 1, 2, 2, 2,  2,  2,  1;
		
		hms::Mesh mesh { std::move(verts) };
		for (Index i {0}; i<mesh.nVertices()-1; ++i)
			REQUIRE( pseudoangle( mesh, i ) < pseudoangle( mesh, i+1 ) );
	}
}


TEST_CASE( "Calculating the component-wise average of vertices" ){
	{
	Matrix2Xs verts1 (2, 4);
	verts1 <<
		0, 1, 1, 0,
		0, 0, 1, 1;
	Vector2s avg1 { 0.5, 0.5 };
	
	Matrix2Xs verts2 (2, 4);
	verts2 <<
		-4, 9,11, 2,
		 3,-6, 1, 9;
	Vector2s avg2 { 4.5, 1.75 };

	SECTION("four vertices, passing vertices directly")
	{
		REQUIRE( vectorAverage(verts1) == avg1 );
	}
	SECTION("four vertices, access through mesh")
	{
		hms::Mesh mesh1 { std::move(verts1) };
		REQUIRE( approxEqual( vectorAverage( mesh1, IdxVector{0,1,2,3} ), avg1 ) );
		
		hms::Mesh mesh2 { std::move(verts2) };
		REQUIRE( approxEqual( vectorAverage( mesh2, IdxVector{0,1,2,3} ), avg2 ) );
		REQUIRE( approxEqual( vectorAverage(mesh2, Array4i{0,1,2,3}), avg2 ) );
	}
	}{
	Matrix2Xs verts1 (2, 3);
	verts1 <<
		0, 1, 2,
		3, 4, 5;
	Vector2s avg1 { 1.0, 4. };
	SECTION("three vertices, passing vertices directly")
	{
		REQUIRE( approxEqual( vectorAverage(verts1), avg1 ) );
	}
	SECTION("three vertices, access through mesh")
	{
		hms::Mesh mesh1 { std::move(verts1) };
		REQUIRE( approxEqual( vectorAverage(mesh1, Vector3i{0,1,2}), avg1 ) );
	}
	}
}



TEST_CASE( "The two possible centroids of a square from an edge are computed",
	"[edgeToSquareCenters]")
{
	using hms::edgeToSquareCenters;
	
	/* going from x-axis counter-clockwise */
	Matrix2s m;
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{ 1, 0} );
	REQUIRE( (m.col(0) - Vector2s{ 0.5, 0.5}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{ 0.5,-0.5}).norm() == Approx(0) );
	
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{ 1, 1} );
	REQUIRE( (m.col(0) - Vector2s{ 0.0, 1.0}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{ 1.0, 0.0}).norm() == Approx(0) );
	
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{ 0, 1} );
	REQUIRE( (m.col(0) - Vector2s{-0.5, 0.5}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{ 0.5, 0.5}).norm() == Approx(0) );
	
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{-1, 1} );
	REQUIRE( (m.col(0) - Vector2s{-1.0, 0.0}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{ 0.0, 1.0}).norm() == Approx(0) );
	
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{-1, 0} );
	REQUIRE( (m.col(0) - Vector2s{-0.5,-0.5}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{-0.5, 0.5}).norm() == Approx(0) );
	
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{-1,-1} );
	REQUIRE( (m.col(0) - Vector2s{ 0.0,-1.0}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{-1.0, 0.0}).norm() == Approx(0) );
	
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{ 0,-1} );
	REQUIRE( (m.col(0) - Vector2s{ 0.5,-0.5}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{-0.5,-0.5}).norm() == Approx(0) );
	
	m = edgeToSquareCenters( Vector2s{0,0}, Vector2s{ 1,-1} );
	REQUIRE( (m.col(0) - Vector2s{ 1.0, 0.0}).norm() == Approx(0) );
	REQUIRE( (m.col(1) - Vector2s{ 0.0,-1.0}).norm() == Approx(0) );
}

TEST_CASE( "The area of a polygon is calculated")
{
	using hms::verticesToArea;
	
	Matrix2Xs verts1 (2, 4);
	verts1 <<
		0, 2, 2, 0,
		0, 0, 2, 2;
	scalar area1 {4};
	
	Matrix2Xs verts2 (2, 4);
	verts2 <<
		0, 4, 2, 0,
		0, 0, 2, 2;
	scalar area2 {6};
	
	SECTION("quadrilaterals, passing vertices directly")
	{
		REQUIRE( verticesToArea(verts1) == area1 );
		REQUIRE( verticesToArea(verts2) == area2 );
	}
	SECTION("quadrilaterals, access through mesh")
	{
		hms::Mesh mesh1 { std::move(verts1) };
		hms::Mesh mesh2 { std::move(verts2) };
		REQUIRE( verticesToArea(mesh1, Array4i{0,1,2,3}) == area1 );
		REQUIRE( verticesToArea(mesh2, Array4i{0,1,2,3}) == area2 );
		REQUIRE( verticesToArea(mesh1, IdxVector{0,1,2,3}) == area1 );
		REQUIRE( verticesToArea(mesh2, IdxVector{0,1,2,3}) == area2 );
	}
	Matrix2Xs verts3 (2, 3);
	verts3 <<
		0, 4, 2,
		0, 0, 4;
	scalar area3 {8};
	SECTION("triangles, passing vertices directly")
	{
		REQUIRE( verticesToArea(verts3) == area3 );
	}
	SECTION("triangles, access through mesh")
	{
		hms::Mesh mesh3 { std::move(verts3) };
		REQUIRE( verticesToArea(mesh3, Vector3i{0,1,2}) == area3 );
		REQUIRE( verticesToArea(mesh3, IdxVector{0,1,2}) == area3 );
	}
}


TEST_CASE( "Calculating the centroid of polygons" ){
	
	using hms::verticesToCentroid;
	
	Matrix2Xs verts1 (2, 4);
	verts1 << /* parallelepiped */
		1, 1, 3, 3,
		1, 3, 5, 3;
	Vector2s centroid1 {2,3};
	
	Matrix2Xs verts2 (2, 4);
	verts2 << /* rectangle */
		0, 0, 2, 2,
		0, 4, 4, 0;
	Vector2s centroid2 {1, 2};
	
	Matrix2Xs verts3 (2, 3);
	verts3 << /* triangle */
		0, 1, 2,
		0, 4, 2;
	Vector2s centroid3 {1, 2};
	
	SECTION( "Quadrilaterals, directly passing matrix" ){
		REQUIRE( verticesToCentroid( verts1 ) == centroid1 );
		REQUIRE( verticesToCentroid( verts2 ) == centroid2 );
	}
	SECTION( "Quadrilaterals, accessing vertices through mesh" ){
		hms::Mesh mesh1 { std::move(verts1) };
		hms::Mesh mesh2 { std::move(verts2) };
		REQUIRE( verticesToCentroid( mesh1, IdxVector{0,1,2,3} ) == centroid1 );
		REQUIRE( verticesToCentroid( mesh2, IdxVector{0,1,2,3} ) == centroid2 );
	}
	SECTION( "Triangles, directly passing matrix" ){
		REQUIRE( verticesToCentroid( verts3 ) == centroid3 );
	}
	SECTION( "Triangles, accessing vertices through mesh" ){
		hms::Mesh mesh3 { std::move(verts3) };
		REQUIRE( verticesToCentroid( mesh3, IdxVector{0,1,2} ) == centroid3 );
	}
}

TEST_CASE( "Intersection of two line segments" ){
	using hms::intersects;
	

	auto req = []( scalar x1, scalar y1, scalar x2, scalar y2, bool res ){
		REQUIRE(
			intersects(
				/* horizontal line at x-axis */
				Vector2s{0,0}, Vector2s{1,0},
				/* other line depends on input */
				Vector2s{x1,y1}, Vector2s{x2,y2}
			) == res
		);
	};

	req(0,-1, 0, 1, true );
	req(0, 1, 0,-1, true );
	req(0, 1, 0, 2, false);
	req(0, 2, 0, 1, false);
	req(2, -1, 2, 1, false);
	Vector2s
		A {0,0}, B{4,4}, C{1,1}, D{2,2}, E{2,1}, F{3,2}, G{3,1}, H{1,3};
	REQUIRE( !intersects(A,B,C,D) );
	REQUIRE( !intersects(A,B,E,F) );
	REQUIRE(  intersects(A,B,G,H) );

	// auto
	// 	AB { ParamLine::Through(A,B) };
	// auto
	// 	CD { Line::Through(C,D) },
	// 	EF { Line::Through(E,F) },
	// 	GH { Line::Through(G,H) };

	// auto print = [&](const auto& l1, const auto& l2, std::string_view msg){
	// 	scalar p { l1.intersectionParameter(l2) };
	// 	std::cout << msg
	// 	<< ", Intersection param: " << p
	// 	<< ", intersection point: " << l1.intersectionPoint(l2).transpose()
	// 	<< ", point at intersection: " << l1.pointAt(p).transpose()
	// 	<< '\n';
	// };
	// print(AB, CD, "AB, CD");
	// print(AB, EF, "AB, EF");
	// print(AB, GH, "AB, GH");
}

TEST_CASE( "Mirroring a point along a line" ){

	auto mirrorAlongX = []( scalar x, scalar y ){
		auto line { ParamLine::Through( Vector2s{0,0}, Vector2s{1,0} ) };
		Vector2s p2 { x, -y };
		REQUIRE( mirrorAlongLine(line, Vector2s{x,y}).isApprox(p2) );
	};
	auto mirrorAlongY = []( scalar x, scalar y ){
		auto line { ParamLine::Through( Vector2s{0,0}, Vector2s{0,1} ) };
		Vector2s p2 { -x, y };
		REQUIRE( mirrorAlongLine(line, Vector2s{x,y}).isApprox(p2) );
	};
	auto mirrorDiagonally = []( scalar x, scalar y ){
		auto line { ParamLine::Through( Vector2s{0,0}, Vector2s{1,1} ) };
		Vector2s p2 { y, x };
		REQUIRE( mirrorAlongLine(line, Vector2s{x,y}).isApprox(p2) );
	};

	std::vector<scalar> pointsToMirror {
		 0,  1,
		 0,  2,
		 1, -5,
		-3,  2,
		-2, -4
	};

	auto mirror = [&]( auto* func ){
		for ( unsigned i{0}; i<pointsToMirror.size(); i+=2 ){
			func( pointsToMirror[i], pointsToMirror[i+1] );
		}
	};

	SECTION("Mirror along x-axis"){
		/* the unary + is to cast the lambda to a function pointer */
		mirror( +mirrorAlongX );
	}
	SECTION("Mirror along y-axis"){
		mirror( +mirrorAlongY );
	}
	SECTION("Mirror along angled line"){
		mirror( +mirrorDiagonally );
	}
	SECTION("Mirroring along edges"){
		/* essentially the same. the three edges run along the x- and y-axis
		 * and the angle bisector of the first quadrant, respectively */
		Matrix2Xs vertices (2,4);
		vertices <<
			0, 1, 0, 1,
			0, 0, 1, 1;
		
		Array2Xi edges (2,3);
		edges <<
			0, 0, 0,
			1, 2, 3;

		Mesh mesh { std::move(vertices), std::move(edges), bool{} };

		for ( unsigned i{0}; i<pointsToMirror.size(); i+=2 ){
			REQUIRE( mirrorAlongEdge(mesh.edge(0),
					Vector2s{pointsToMirror[0],pointsToMirror[1]}
				).isApprox(
					Vector2s{pointsToMirror[0],-pointsToMirror[1]}
				)
			);
			REQUIRE( mirrorAlongEdge(mesh.edge(1),
					Vector2s{pointsToMirror[0],pointsToMirror[1]}
				).isApprox(
					Vector2s{-pointsToMirror[0],pointsToMirror[1]}
				)
			);
			REQUIRE( mirrorAlongEdge(mesh.edge(2),
					Vector2s{pointsToMirror[0],pointsToMirror[1]}
				).isApprox(
					Vector2s{pointsToMirror[1],pointsToMirror[0]}
				)
			);
		}
	}
	SECTION( "Precalculated values" ){
		/* c* is the centroid of a cell, t* is the target, which here is the
		 * the centroid mirrored along a boundary edge */
		Vector2s
			c0 { 1, 1 },
			c1 { 0, 2 },
			c2 { -4./3, 4./3 },
			c3 { -5./3,-1./3 },
			t0 { 1, -1 },
			t1 { -1./3, -5./3 },
			t2 { -9.8/3, -2.6/3 },
			t3 { -8./3, 8./3 },
			t4 { 0, 4 },
			t5 { 2.2, 1.4 };
		
		Mesh mesh { testMeshTriangles(true) };
		REQUIRE( mirrorAlongEdge(mesh.edge(3), c0).isApprox(t0) );
		REQUIRE( mirrorAlongEdge(mesh.edge(4), c3).isApprox(t1) );
		REQUIRE( mirrorAlongEdge(mesh.edge(5), c3).isApprox(t2) );
		REQUIRE( mirrorAlongEdge(mesh.edge(6), c2).isApprox(t3) );
		REQUIRE( mirrorAlongEdge(mesh.edge(7), c1).isApprox(t4) );
		REQUIRE( mirrorAlongEdge(mesh.edge(8), c0).isApprox(t5) );
	}
}

TEST_CASE( "Coordinate transformation" ){
	SECTION("Single vector"){
		Vector2s v {1, 0};
		SECTION( "Using normal vector directly" ){
			Vector2s n;
			n = {0, 1};
			REQUIRE( Vector2s{transformCoords(v,n)}.isApprox(Vector2s{0, -1}) );
			n = {1, 0};
			REQUIRE( Vector2s{transformCoords(v,n)}.isApprox(v) );
			n = Vector2s{1, 1}.normalized();
			REQUIRE( Vector2s{transformCoords(v,n)}.isApprox(
				Vector2s{1,-1}.normalized()
			) );
		}
		SECTION( "Edge overload" ){
			Mesh mesh { testMeshRectangles(true) };
			/* horizontal edge (normal pointing up)*/
			REQUIRE( Vector2s{transformCoords(v, mesh.edge(0))}.isApprox(Vector2s{0, -1}) );
			/* vertical edge (normal pointing to the right) */
			REQUIRE( Vector2s{transformCoords(v, mesh.edge(2))}.isApprox(v) );
			mesh = testMeshTriangles(true);
			/* diagonal edge (normal pointing leftwards and up) */
			REQUIRE( Vector2s{transformCoords(v, mesh.edge(6))}.isApprox(
				Vector2s{-1,-1}.normalized()
			));
		}
	}
	SECTION("Multiple columns"){
		Array2Xs v (2, 5), n, vt, res;
		v <<
			1, 2,  1, -2, 3,
			2, 1, -2,  1, 4;
		n.resizeLike(v);
		n <<
			0, 1, 1,  0, -1,
			1, 0, 1, -1,  1;
		n.colwise().normalize();

		vt.resizeLike(v);
		scalar s { 1.0/sqrt(2) };
		vt <<
			 2, 2, s*(-1), -1, s,
			-1, 1, s*(-3), -2, s*(-7);

		res = transformCoords(v,n);

		REQUIRE( res.isApprox(vt) );
		REQUIRE( retransformCoords(res,n).isApprox(v) );
	}
}